# -- coding:utf-8 --
#!/usr/bin/python
import sys, os
sys.path.append("/home/avatar/Minibot/04.RobotSamples/02.CarRobot")
import inspect
from BwSensorLib import BwSensorLib
from CarRobot import CarRobot
from BwRobotLib import BwRobotLib
from MiniBotControlLib import MiniBotControlLib
import time
import BwHelpLib 

msg = """
请选择轮子模式 :
2 : 两轮
3 : 三轮
4 : 四向轮
5 : 四麦克朗姆轮
6 : 四并橡胶轮
CTRL-C to quit
"""

cmd = """
移动小车控制命令
↑ : 向前走  ↓ : 向后走
← : 左转    → : 右转
s : 停止
a : 左走（5 : 四麦轮）
d : 右走（5 : 四麦轮）
"""

class CarKeyboardCtl:
    def __init__(self):
        # 实例化MiniBotControlLib类，并设置为udp模式
        self.mlib = MiniBotControlLib("udp")
        # 输入ip地址和端口号
        self.ip = "10.10.100.254"
        self.port = 8899
        # 连接机器人,并设置超时时间2s
        self.mlib.Connect(self.ip, self.port, 2000)
        # 实例化BwRobotLib类，并将mlib作为参数传入
        self.robotlib = BwRobotLib(self.mlib)
        # 实例化carRobot类，并将mlib作为参数传入
        self.carRobot = CarRobot(self.mlib)
        # 实例化传感器库对象
        self.sensorlib = BwSensorLib(self.mlib)
        # 根据模块连接位置，自动加载ID配置,print_flag设为True，可打印打印当前模块信息
        self.robotlib.module_ids = self.robotlib.scanNConfigCarIds(print_flag=True)
        # 配置轮式模式 2:两轮   3:三轮   4:四向轮  5:四麦克朗姆轮  6:四并橡胶轮
        print(msg)
        self.wheel_mode = BwHelpLib.inputNum()
        
        # 配置机器人（包括ID位置与速度模式）
        # 配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】
        self.carRobot.bindIds(self.robotlib.module_ids, self.wheel_mode)
        # 设置小车速度
        self.speed = 50
        self.carRobot.update_speed(self.speed)


    def handleControlKeys(self, cmd):
        if(cmd == '\x10'):
            self.carRobot.goForward()
        if(cmd == '\x11'):
            self.carRobot.goBack()
        if(cmd == 's'):
            self.carRobot.stop_wheel()
        if(cmd == '\x13'):
            self.carRobot.turnLeft()
        if(cmd == '\x12'):
            self.carRobot.turnRight()
        if(cmd == 'a'):
            self.carRobot.runLeft()
        if(cmd == 'd'):
            self.carRobot.runRight()


if __name__ == "__main__":
    try :
        car = CarKeyboardCtl()    
        print(cmd)
        while(1):
            key = BwHelpLib.readkey()
            car.handleControlKeys(key)     
            if (key == '\x03'): # Ctrl+C 退出程序
                break        
    except KeyboardInterrupt:
        print(" Program closed! ")
    pass

